#include "TimedSolenoid.h"

TimedSolenoid::TimedSolenoid(uint32_t port) :
	RobotSubsystem()
{
	m_solenoid = new Solenoid(port);
	m_timer = new Timer();
	m_state = kNotTriggering;
}

TimedSolenoid::~TimedSolenoid()
{
	
}

void TimedSolenoid::Trigger( bool on)
{
	if(on){
		m_state = kTriggeringOn;
	} else{
		m_state = kTriggeringOff;
	}
	m_timer->Stop();
	m_timer->Reset();
	m_timer->Start();
}

void TimedSolenoid::DoTriggering(void)
{
	switch (m_state){
	case kNotTriggering:
		break;
	case kTriggeringOn:
		if (m_timer->Get() < c_timeout){
			m_solenoid->Set(true);
		} else {
			m_state = kNotTriggering;
			m_timer->Stop();
		}
		break;
	case kTriggeringOff:
		if(m_timer->Get() < c_timeout){
			m_solenoid->Set(false);
		} else{
			m_state = kNotTriggering;
			m_timer->Stop();
		}
		break;
	}
}

void TimedSolenoid::DoAutonomousPeriodic(void)
{
	DoTriggering();
}

void TimedSolenoid::DoTeleopPeriodic()
{
	DoTriggering();
}
